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ABSTRACT 

Rigid body localization refers to a problem of estimating the position 
of a rigid body along with its orientation using anchors. We consider 
a setup in which a few sensors are mounted on a rigid body. The 
absolute position of the rigid body is not known, but, the relative po- 
sition of the sensors or the topology of the sensors on the rigid body 
is known. We express the absolute position of the sensors as an affine 
function of the Stiefel manifold and propose a simple least-squares 
(LS) estimator as well as a constrained total least-squares (CTLS) 
estimator to jointly estimate the orientation and the position of the 
rigid body. To account for the perturbations of the sensors, we also 
propose a constrained total least-squares (CTLS) estimator. Analyt- 
ical closed-form solutions for the proposed estimators are provided. 
Simulations are used to corroborate and analyze the performance of 
the proposed estimators. 

Index Terms — Rigid body localization, Stiefel manifold, atti- 
tude estimation, tilt estimation, sensor networks. 

1. INTRODUCTION 

Advances in wireless sensor technology and their usage in networks 
have given birth to a variety of sensing, monitoring, and control ap- 
plications. The majority of applications with a wireless sensor net- 
work (WSN) rely on two fundamental aspects: distributed data sam- 
pling and information fusion. For the data to be meaningful it is 
important to know not only the time instance (temporal information) 
at which the data is acquired, but also the location (spatial informa- 
tion) where the data is acquired. Identifying the sensor's location is 
a well-studied topic 1 1 1, and is commonly referred to as localization. 

Localization can be either absolute or relative. In absolute local- 
ization, the nodes are usually localized using a few reference nodes 
whose positions are known. Absolute localization problems are typ- 
ically solved using range-square methods from measurements based 
on certain physical phenomena, e.g., time-of-arrival (TOA) fni2l. 
Localization can also be relative. In relative localization, the aim is 
to identify the topology of the network, and determining the loca- 
tion of the nodes relative to other nodes is sufficient. Classical solu- 
tions to relative localization are based on multi-dimensional scaling 
(MDS) f3,4|. 

In this paper, we provide a new and different flavor of lo- 
calization, called rigid body localization. The problem in rigid 
body localization is to identify the location of the body in a three- 
dimensional space and also the orientation of the body along these 
three-dimensions. Rigid body localization has a huge potential in a 
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variety of different fields. To list a few, it is useful in the areas of 
underwater (or in-liquid) systems, orbiting satellites, mechatronic 
systems, unmanned aircrafts, gaming consoles, or automobiles. In 
such applications, traditional localization of the node(s) is not suffi- 
cient. For example, in an autonomous underwater vehicle (AUV) [51, 
or an orbiting satellite [6 |, the sensing platform is not only subject to 
motion but also to rotation. In such cases, together with positioning, 
determining the orientation of the body also forms a key component, 
and is essential for both controlling (maneuvering) and monitoring 
purposes. 

Commonly the term attitude estimation (for flights and space- 
crafts) or tilt sensing (for industrial equipments and consumer de- 
vices) is used for determining the orientation of the object in a three- 
dimensional space which typically uses inertial sensors |7|, or ac- 
celerometers |8|. However, inertial sensors and accelerometers gen- 
erally suffer from drift errors. On the other hand, in rigid body lo- 
calization we propose to exploit the communication packets con- 
taining the ranging information, just as in traditional localization 
schemes | I|, to estimate both the rotations and the translations. In 
short, we present rigid body localization as an estimation problem 
from a signal processing perspective. 

More specifically, we consider a rigid body on which a few sen- 
sors are mounted. The absolute location of the rigid body itself 
is unknown, but, the relative position of the sensors or the sensor 
topology on the rigid body is known. A novel problem to jointly po- 
sition the rigid body and estimate its orientation using a few nodes 
with known absolute locations (anchors) is considered. For this pur- 
pose, we parameterize the Stiefel manifold |9 1 with a known sensor 
topology and propose a new least-squares (LS) estimator and also 
a constrained least-squares (CLS) estimator. The sensor positions 
are usually perturbed during fabrication of the body or if the body is 
not entirely rigid. To take these perturbations into account, we also 
propose a constrained total least-squares (CTLS) estimator. Analyt- 
ical closed-form solutions for the proposed estimators are provided. 
Simulations are provided to validate and analyze the performance of 
the proposed estimators. 

Notation: Upper (lower) bold face letters are used for inatri- 
ces (column vectors); (■)"^ denotes transposition; diag(.) refers to a 
block diagonal matrix with the matrices in its argument on the main 
diagonal; Ijv (Ojv) denotes the A'^ x 1 vector of ones (zeros); Ijv 
is an identity matrix of size A^; E(.) denotes the expectation oper- 
ation; ® is the Kronecker product; (.)^ denotes the pseudo inverse, 
i.e., for a full column-rank matrix A the pseudo inverse is given 
by — (A'^A^^)A"^; vec(.) is a vector formed by stacking the 
columns of its matrix argument; vec~^(.) is a matrix formed by the 
inverse vec(.) operation. 
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Fig. 1: An illustration of the sensors on a rigid body undergoing a 
rotation and a translation. 

2. PROBLEM FORMULATION 

2.1. The model and preliminaries 

Consider a network with A/ anchors (nodes with known absolute lo- 
cations) and TV sensors in a 3-dimensional space. The sensors are 
mounted on a rigid body (e.g., a plane or a pyramid) as illustrated in 
Fig. [T] The relative position of these sensors or its topology on the 
rigid body is known up to a certain accuracy. However, the absolute 
position of the sensors or the rigid body itself in the 3-dimensional 
space is not known. The rigid body experiences rotations and trans- 
lations in each dimension. 

The sensors are mounted on the rigid body (e.g., in the factory) 
and the topology of how these sensors are mounted is known. In 
other words, we can connect a so-called reference frame to the rigid 
body, as illustrated in Fig. 1, and in that reference frame, the co- 
ordinates of the nth sensor are given by the known 3x1 vector 
Cn = [cn.i, c„,2, c„,3]"^. So the seusor topology is basically de- 
termined by the matrix C — [ci, C2, . . . , cjv] G R''^'^. Let the 
absolute coordinates of the mth anchor and the nth sensor be de- 
noted by a 3 X 1 vector a™ and s,i, respectively. These absolute 
positions of the anchors and the sensors are collected in the matri- 
ces A = [ai,a2, . . .,slm] G R^^'^^ and S = [si,S2, . . . ,S]v] G 
R^'^^, respectively. 

The pairwise distance between the mth anchor and the nth sen- 
sor is denoted by r(am,s„) = ||am — s„||2 and is typically ob- 
tained from ranging |[T]|2][T0). The range measurements can be ex- 
pressed as 



(1) 



where em,n is the additive stochastic noise resulting from the rang- 
ing process. Assuming TOA-based ranging, we model emn as an 
i.i.d. zero mean white random process with a variance a^{emn) = 

jr~K ^ ^snr"^^ nil . Here, c is the speed of a wave in a medium, 
Ns is the number of samples used in the ranging process, k is a 
constant, and SNR is the signal-to-noise ratio of the range measure- 
ments. The r^{sLm., s„) tem in the variance is due to the path-loss 
model assumption and penalizes the range measurements based on 
distance. Since all the sensors are mounted on the rigid body, it is 
reasonable to assume that all sensors experience approximately the 



same path-loss, especially when the anchors are far away from the 
rigid body. Hence, we use a simplified noise model with variance 



3e^ r^(an^,si) 
NsK SNR 



(2) 



Here, we choose sensor si just for illustration purposes, and in prin- 
ciple, this can be any sensor. 

We can now write the squared pairwise distance between the 
mth anchor and the nth sensor as 



T 



(3) 



and 



(4) 

where nm,n ~ 2r(am, si)e,„.„ + e^„.„ is the new noise term ob- 
tained due to squaring. We can compute the mean E(nm,n) ~ and 
the variance = E(n^ „) ~ ia'^ {em,n)r^ {s.m, si), ignoring the 
higher-order terms under the condition of sufficiently small errors. 

Defining the Mx 1 vectors d(s„) = [d(ai, s„), . . . , d(aAf , s„)]"^ 
and u — [||ai 11^, ||a2 Ip, . . . , ||aA/ 11^]"^, we can now write the 
squared pairwise distances of the nth sensor to each anchor in a 
vector form as 



d(s„ 



+ IISnll'^lM + n(s„ 



(5) 



where n(s,i) = [ni^„, n2,n, • • • , nM,n]'^ G R^"'^'^''" is the error vec- 
tor. The M X M covariance matrix of the error vector n(s„) will 
be S„ — diag(crf , al, . . . , o-\i). We whiten ([5} to obtain an iden- 
tity noise covariance matrix by multiplying both sides of (|5j with 



W G R'*^''*^ which leads to 



Wd(s„) = W(u-2A^s„ + ||s„||nA/+n(s„)) (6) 



The optimal W is W* 



-1/2 



but depends on the unknown 



^ 1/2 ^ 1/2 

r(am, si). Hence, we use W = S„ , where S„ is computed 

using f(am,si). 

In order to eliminate ||s„||'^ and thus the vector WIa/, the 

conventional technique is to apply an orthogonal projection matrix 



such that Pa/W1 



0. 



iJjWWlM 

However, this would again color the noise. To avoid this, we propose 
to use a unitary decomposition of Pm, i.e., 'Pm = UAfUj/ where 
\J M is a M X (Af — 1) matrix obtained by collecting orthonormal 
basis vectors of the null-space of Wl m so that U^/ Wl m = 0. 

In order to eliminate the ||s„|PW1a/ term in ([6j without col- 
oring the noise, we left-multiply both sides of ([5} with Ua/ , which 
leads to 

Ul,W(d(s„) - u) = - 2Ul,WA^s„ + Ul,Wn(s„), (7) 

We can now stack (O for all the sensors as 

UlrWD = -2Ul}WA^S + Uj^WN (8) 

where D — [d(si ),..., d(sjv)]—ulj^ and N = [n(si), • • ■ , n(sjv) 
are both M x N matrices. The approximation in (|2j allows this 
stacking by using a common whitening matrix W for all the sen- 
sors. In addition, the covariance matrix of vec(UA/WN) will be 
approximately I(a/-i)]v- 



2.2. Sensor topology on the Stiefel manifold 

A Stiefel manifold |9| in three dimensions, commonly denoted by 
V3,3, is tlie set of all 3 x 3 unitary matrices Q = [qi, q2, qa] £ 
E^'''^ i.e., V3,3 = {Q e R^""^ : Q^Q = I3}. The absolute 
position of the nth sensor can be written as an affine function of a 
point on the Stiefel manifold, i.e.. 



Sn = c„,iqi + C„,2q2 + C„,3q3 + t 

= Qc„ + 1 

j3xl 



(9) 



(10) 



where t £ R denotes the translation and is unknown. Note that 
the combining weights c„ are equal to the known coordinates of the 
nth sensor in the reference frame, as introduced in Section |27T] This 
means that the unknown unitary matrix Q actually tells us how the 
rigid body has rotated in the reference frame. 
We can further stack (|9) for all the sensors as 

Q 

s = Qc + ti?; = foT^ [-^ 

Note that in l llOt . we express the unknown sensor locations S in 
terms of the unknown rotations Q of a known sensor topology C 
and an unknown translation t. 

3. THE PROPOSED ESTIMATORS 

In this paper, we consider the novel problem to localize the rigid 
body by estimating the rotations Q and translations t in each dimen- 
sion relative to the anchors. The matrix Q forms an orthonormal 
basis for the subspace spanned by the rigid body which reveals all 
the rotations. 

3.1. LS estimator (Unconstrained) 

Combining ([8} and dlOt results in the following linear model 



uTfWD 



which can be written as 



-2Ul,WA^Qea + UliWN (11) 



D = AQeCe+N (12) 

where D ^ Ul.WD G R^''-^^'<'', A ^ -2Ui',WA^ G 
^(M-i)x:i^ and N = U^^WN G R(*^-i)xiv^ fyj-jher 

vectorize ( I12t as 

d = (Cl' ® A)q, + n (13) 

where qe = vec(Qe) = [qf , qT, qif, t^^^ 
vec(D) G R("-i)^x\ and n = vec(N) G 1 

To jointly estimate the unknown rotations Q and the translations 
t we propose the following joint LS estimator 



G R'"^\ d 

(A/-l)JVxl 



qe,LS = (Cf ® A)td 



(14) 



which will have a unique solution if 8 A has full column-rank 
which requires {M ~ 1)N > 12. Finally, we have Qe.iS = 
vec^^ (qe,_Ls) = [ Qls \ ^ls ]■ Note that since n is approxi- 
mately white, we do not use any weighting in the LS formulation. 

3.2. Unitarily constrained LS estimator (CLS) 

The solution of the unconstrained LS estimator does not necessarily 
lie in the set V3,3, i.e., the columns of the LS estimate Qls obtained 
in il4\ are generally not orthogonal to each other and they do not 
have a unit norm. 

We next propose a LS estimator with a unitary constraint on Q. 
For this purpose, we decouple the rotations and the translations in 



dipt . For this purpose, we adopt a unitary decomposition of Pjv = 
Ijv - jflNi-Jf, i.e., Piv = UjvU^ where Uiv is a A'' x (A^ - 1) 
matrix obtained by collecting orthonormal basis vectors of the null- 
space of Ijv so that l^Ujv = 0. Right-multiplying Ujv to both 
sides of (To) leads to 

SUjv = QCUjv. (15) 
Combining ^ and l llSt we get the following linear model 

UmWDUjv = -2UmWA^QCUjv + Ul/WNUiv (16) 
which can be written as 



D = AQC + N 



(17) 
CUjv, and 



where D = UjfWDUjv, A : 
N = UjjWNUjv. As before, the covariance matrix of vcc(N) 
will be approximately I(jv/-i)(]v-i)- 

To estimate Q we propose a LS problem with a quadratic equal- 
ity constraint as given by 



mm 

q 

s.t. 



IQC-XII 



Q^Q 



(18) 



I3 



where X = A^D assuming that A has full column-rank. The opti- 
mization problem in ( I18t is non-convex due to the quadratic equality 
constraint and is commonly referred to as the orthogonal Procrustes 
problem {OPP)\l2\. 

Remark 1 (Anchor positioning). For M > 3, the anchor positions 
can be designed such that the matrix A will be full column-rank and 
well-conditioned. Then, the matrix A is left-invertible, i.e., A^ A — 
I3 

Lemma 1 (Solution to unitarily constrained LS 1 12 1). The con- 
strained LS problem in il8l has an analytical solution Qcls = 
VU"^ where U and V are obtained from the singular value decom- 
position (SVD) o/CX-^ which is given by USV^. 

Subsequently, the LS estimate of the translations t can be com- 
puted by using Qcls in dlOl ) and dl2b as 



icLS = ^(A^D 



QcLsC)li 



(19) 



Remark 2 (Weighted orthogonal Procrustes problem [13]). The 
pseudo inverse operation in dicSD would color the noise. This can 
be avoided by solving a weighted orthogonal Procrustes problem. 
This does not have a closed-form solution, but can be solved using 
Newton iterations U3V . 

3.3. Unitarily constrained TLS estimator (CTLS) 

In the previous section, we assumed that the sensors are mounted 
on a rigid body and their topology is accurately known. In practice, 
there is no reason to believe that errors are restricted only to the range 
measurements and there are no perturbations on the initial sensor 
positions. The perturbations can be introduced during fabrication of 
the rigid body or if the body is not entirely rigid. 

The position of the nth sensor in the reference frame c„ is noisy. 
We denote the perturbation on c„ as S„, and the perturbations on C 
as A = [Ji,d2,...,5jv]Ujv. Taking the perturbations of the sensor 
into account we can re-write the data model in ( 117) as 



Q(C + A) = X + E 



(20) 



where E = A^N and Q is to be determined as earlier 

The solution to the data model in jlOi leads to the classical TLS 
optimization problem but with a unitary constraint. The unitarily 
constrained TLS optimization problem is given by 



mm 

q 



s.t. 



Q(C + A) 
Q^Q = I3 



IE 



|2 
If' 



(21) 



Lemma 2 (Solution to unitarily constrained TLS fT4\). The unitar- 
ily constrained TLS problem in l l2j| ) has the same solution as the uni- 
tarily constrained LS problem, and the solution is Q,ctls = VU"^. 

The algorithms to compute the solution for a unitarily con- 
strained LS and TLS are summarized as Algorithm [T] 

Algorithm 1 Unitarily constrained LS or TLS 

Require: CX^,X = AtD. 
Compute: SVD CX^ = USV^ 

9 = vu^ 

t = i(AtD-QC)liv 



4. NUMERICAL RESULTS 

We consider six sensors mounted on a rigid pyramid as shown in 
Fig. [T] The coordinates of the sensors in the reference frame are 
chosen such that, 



1 6 7 6 2 2.5 
5 5 5 2.5 
5 



(22) 



and M = 10 anchors are deployed uniformly at random in a range 
of 100 m. We use a rotation of (20, —25, 10) degrees in each di- 
mension, which determines Q, and a translation of 5 m along each 
dimension. The simulations are averaged over N^xp = 1000 inde- 
pendent Monte-Carlo experiments. 

We analyze the performance of the three proposed estimators: 
1) LS (unconstrained), 2) unitarily constrained LS, and 3) unitar- 
ily constrained TLS. The performance of the estimators for estimat- 
ing the rotations are provided in terms of the mean angular error 



defined as 



This is shown 



Ittw 



is shown in 



iVcxp ■ "112 

m = 1, 2, 3 and t''^ are the parameters estimated 



in Fig. |2] The root mean square error (RMSE) for estimating the 
corresponding translations 
Fig.m Here, 

during the ith Monte-Carlo experiment. Note that in case of the un- 
constrained LS estimator ||qm II2 7^ 1- 

Simulations are provided for various reference ranges defined as 
10 logio ^"afii^^ dB. A reference range of 100 dB means that for 
a range of 100 m the standard deviation on the estimated range is 
1 mm. The range measurements in ([T) are corrupted with an i.i.d. 
Gaussian random process of ) derived for the cor- 

responding reference range. 

For the perturbed case, the sensor topology is corrupted with a 
zero mean i.i.d. Gaussian process with a standard deviation of 1 mm. 
The performance of the unconstrained and constrained estimators in 
case of perturbations is shown in Fig.|2b]and Fig.|3b| 
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Fig. 2: Mean angular error of the estimated rotations Q. 
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Fig. 3: RMSE of the estimated translations t. 

5. CONCLUSIONS 

We have proposed a problem called rigid body localization, in which 
the aim is to jointly localize and estimate the orientation of a rigid 
body in a 3-dimensional space. For rigid body localization, we make 
use of a few anchors and a known sensor topology of sensors that 
are mounted on the rigid body. We parameterize the Stiefel man- 
ifold using the known sensor topology and propose unconstrained 
and constrained LS estimators. In order to take the perturbations of 
the sensor into account, we also propose a unitarily constrained TLS 
estimator. Incidentally, the solutions to both the constrained LS and 
constrained TLS estimators are the same. Analytical closed-form 
solutions for all the estimators have been provided. 
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